classdef Helper< handle
    %UNTITLED12 此处显示有关此类的摘要
    %   此处显示详细说明
    
    properties (Constant)
        ww = 2;
        wh = 2;
        
        trueW = 240 - Helper.ww;
        trueH = 150 - Helper.wh;
        whRatio = Helper.trueW / Helper.trueH;
        
% %         ImgName = '';
% %         DataPath = '';
        args = {};
    end
    
    methods(Static = true)
        % % % % % % % % % % % % % % %测试用
        function test()
            img = imread('test.png');
            imshow(img);
            
            BW =  img > 200;
            stats = regionprops('table', BW, 'Area', 'MajorAxisLength', 'MinorAxisLength', 'Centroid', 'Orientation', 'BoundingBox');
            Centroid = stats.Centroid;
            major = stats.MajorAxisLength;
            minor = stats.MinorAxisLength;
            hold on
            plot(Centroid(1), Centroid(2), 'xr');
            xx = [Centroid(1) - major/2, Centroid(1) + major/2];
            yy = [Centroid(2), Centroid(2)];
            plot(xx, yy, 'b-');
            
            xx = [Centroid(1), Centroid(1)];
            yy = [Centroid(2)  - minor/2, Centroid(2) + minor/2];
            plot(xx, yy, 'b-');
            
            hold off
        end
        
        % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
        % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
        % % % % 获得方尺内部的图像
        function BW7 = findInnerRegion(lll, bShow)
            % % % 利用阈值与形态学 把内部与外部区域变黑，把尺子部分变亮，后二值化
            I = rescale(lll);
            %     figure,imshow(I)
            BW = imbinarize(I);
            %     figure,imshow(BW)
            
            w = 2;
            BW3 = Helper.DoMorphByW(BW, w);
            
            BW4 = Helper.RemoveWhiteNoise(BW3);
            if bShow
                figure,montage({BW, BW3, BW4}, 'Size', [1, 3]),title('去掉小亮白区域');
            end
            
            BW5 = imcomplement(BW4);
            BW5 = Helper.RemoveBlackNoise(BW5);
            
            if bShow
                figure,montage({BW, BW4, BW5}, 'Size', [1, 3]),title('去掉小亮白区域，只留两个大白');
            end
            
            BW6 = Helper.RemoveOuterNoise(BW5);
            
            if bShow
                figure,montage({BW, BW5, BW6}, 'Size', [1, 3]),title('去掉小黑区域，只留一个大白');
            end
            w = 128;
            BW7 = Helper.DoMorphByW(BW6, w);
            if bShow
                figure,montage({BW, BW6, BW7}, 'Size', [1, 3]),title('腐蚀膨胀');
            end
        end
        
        
        function BW3 = DoMorphByW(BW, w)
            type = 'square';
            se = strel(type, w);
            
            BW2 = imclose(BW, se);
            BW3 = imopen(BW2, se);
        end
        
        function BWdenoise = RemoveWhiteNoise(BW)
            stats = regionprops('table', BW, 'Area');
            area = stats.Area;
            thread = max(area) - 1;
            
            BWdenoise = bwareaopen(BW, thread);
        end
        
        %% 只剩两个白色的区域，以后可以直接应用RemoveOuterNoise来操作，不用这么麻烦 2022-01-22
        function BWdenoise = RemoveBlackNoise(BW)
            stats = regionprops('table', BW, 'Area');
            area = stats.Area;
            
            [ss,ins]=sort(area, 'descend');
            thread = ss(2);
            
            BWdenoise = bwareaopen(BW, thread);
        end
        
        function BWdenoise = RemoveOuterNoise(BW)%%简单一点，判断中点是否为白色，2022-01-02
            % % %             stats = regionprops('table', BW, 'Area', 'PixelIdxList', 'BoundingBox');
            % % %             boxes = stats.BoundingBox;
            % % %
            % % %             area = (boxes(:, 4) - boxes(:, 2)) .* (boxes(:, 3) - boxes(:, 1));
            % % %
            % % %             [M,ins] = max(area);  %% 外围面积大，要去除
            % % %             BWdenoise = BW;
            % % %             idList = stats.PixelIdxList{ins};
            % % %             BWdenoise(idList) = 0;
            
            cc = bwconncomp(BW);
            mat = labelmatrix(cc);
            
            szImg = size(BW);
            cent = round(szImg / 2);%%注意是（y,x)形式
            idx = mat(cent(1), cent(2));
            BWdenoise = ismember(mat,idx);
        end
        % % % % % % % % % % % % % % % % % % % % % % % % %
        % % % % % % % % % % % % % % % % % % % % % % % % %
        function [Jproj, ptLoc, distM, ratio, mmPerPixel] = findConnersAndImage(BW, img8, bShow)
            I = BW;
            corners = detectHarrisFeatures(I, 'MinQuality', 0.1, 'FilterSize', 65);%%要21以上，保证第4角点比第4角点要显著分别
            
            if bShow
                figure,imshow(I),title('掩膜BW')
                figure,image(img8),title('原图加4角点'); hold on;
                plot(corners.selectStrongest(4));
                axis equal;
                axis tight;
                hold off
            end
            
            aa = corners.selectStrongest(5).Metric;
            if length(aa) == 5
                ratio = aa(4)/aa(5);
            else
                ratio = -1;
            end
            
            ptCorners = corners.selectStrongest(4);
            ptLocTmp = ptCorners.Location;
            
            ptSum = sum(ptLocTmp, 2);  %% 行坐标与列坐标相加
            ptDiff = ptLocTmp(:, 2) - ptLocTmp(:, 1); %% 行坐标 减去 列坐标
            
            [B1,ins] = sort(ptSum);
            [B2,ins2] = sort(ptDiff);
            format short g
            %     ptLoc = ptLoc(ins, :)
            ptLoc(1, :) = ptLocTmp(ins(1), :);%%最小值是左上角
            ptLoc(4, :) = ptLocTmp(ins(4), :);%%最大值是右下角
            ptLoc(2, :) = ptLocTmp(ins2(1), :);%%最小值是右上角
            ptLoc(3, :) = ptLocTmp(ins2(4), :);%%最大值是左下角
            %             ptLoc
            
            cen1 = (ptLoc(1, :) + ptLoc(4, :)) /2;
            cen2 = (ptLoc(2, :) + ptLoc(3, :)) /2;
            center = mean(ptLoc);%%得到中心点的估计坐标
            
            if bShow
                hold on
                plot(cen1(1), cen1(2), 'or')
                plot(cen2(1), cen2(2), 'db')
                plot(center(1), center(2), 'xg')
                hold off
            end
            
            distM = zeros(length(ptLoc));
            for i = 1 : 3
                for j = i+1 : 4
                    distM(i, j) = norm(ptLoc(i,:) - ptLoc(j, :));
                    distM(j, i) = distM(i, j);
                end
            end
            width = (distM(1,2) + distM(3,4)) /2;
            height = (distM(1,3) + distM(2,4)) /2;
            ratioW2H = width/ height;
            
            %% 判断能否成为一个矩阵
            [isValid, iouR] = Helper.judgeIsValidRect(BW, ptLoc);
            if isValid == false || abs(ratioW2H-Helper.whRatio) >0.2
                disp('找不到内部长方形的尺寸')
                disp([iouR, ratioW2H, ratio]);
                Jproj = [];
                return;
            end
            % % % % %
            
            %     width = 1500; %%用真实距离
            rr = 1.6;%%用真实距离的比例  2400:1500
            rr = Helper.whRatio;%%用观察到的形变比例
            if width > height   %%可以不用做判断，已确保图像的宽度大于高度
                width = height * rr;
            else
                height = width * rr;
            end
            fixPts = [0, 0;
                width, 0;
                0, height;
                width, height;
                width/2, height/2];
            ptLoc = [ptLoc; center];%%加上中心点,增加鲁棒性
            
            mmPerPixel = Helper.trueH / height;%%计算每像素对应的mm
            
            tform = fitgeotrans(ptLoc, fixPts,'projective');%%projective,affine, 用投影变换
            
            Jproj= imwarp(img8, tform, 'OutputView',imref2d([ceil(height), ceil(width)]));
            
            if bShow
                figure, imshow(Jproj),title('截取的方尺内部图')%%值截取小图
            end
            
            %      画出转换坐标后的大图，OK
            % % %                  Jregistered = imwarp(img8, tform);
            % % %                  figure, imshow(Jproj)
            % % %                  hold on;
            % % %                  plot(fixPts(:, 1), fixPts(:, 2), '-r')
            % % %                  hold off;
            
            % % %             save(saveName, 'Jproj');
            % % %             imwrite(Jproj, saveImg);
        end
        function [isValid, ratio] = judgeIsValidRect(BW, ptLoc)
            stats = regionprops('table', BW, 'BoundingBox');
            BoundingBox = stats.BoundingBox;
            bbox = BoundingBox(1, :);
             
% %             ptLoc(1, :) = ptLocTmp(ins(1), :);%%最小值是左上角
% %             ptLoc(4, :) = ptLocTmp(ins(4), :);%%最大值是右下角
% %             ptLoc(2, :) = ptLocTmp(ins2(1), :);%%最小值是右上角
% %             ptLoc(3, :) = ptLocTmp(ins2(4), :);%%最大值是左下角
            
            bbox2 = [ptLoc(1, :), ptLoc(4, :) - ptLoc(1, :)];
            
            ratio = bboxOverlapRatio(bbox, bbox2);
            isValid = ratio> 0.6;
        end
        
        % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
        % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
        % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
        % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
        % % % % % % % % % % % % % % % % % % %要确保 图像的宽度大于高度
        %% 提取种子的完整形态,并消噪声，但不影响种子主干
        function BW = ExtractSeeds(img8, bShow)
            rgb = im2double(img8);
            
            I = rgb(:, :, 1) - rgb(:, :, 3);
            BW = imbinarize(rescale(I));  %%2021-01-23 修改
            w = 2;
            BW = Helper.DoMorphByW(BW, w);
            
            gray = rgb2gray(rgb);
            mask1 = imbinarize(gray);
            w = 2;
            mask1 = Helper.DoMorphByW(mask1, w);
            
            mask = mask1 | BW;
            if bShow
                figure,montage({BW, mask}, 'Size', [1, 2])
            end
                      
            w = 15;
            masker = imerode(BW, strel('square', w));
            BW = imreconstruct(masker, mask);%%形态学重建
            if bShow
                figure,imshow(BW),title('形态学重建')
            end
            
            
            
% % % % % % %             2022-01-11
% % % % % % % 消掉 在边界上的大白
            BW2 = mask;
            BW2(1, 1:end) = true;
            BW2(end, 1:end) = true;
            BW2(1:end, 1) = true;
            BW2(1:end, end) = true;
            BW2 = BW2 | BW;
            
            cc = bwconncomp(BW2);
            dd = labelmatrix(cc);
            inx = dd(1,1);
            PixelIdxList = cc.PixelIdxList{inx};
            BW(PixelIdxList) = false;
            
            BW = imfill(BW,'holes');%%进行孔洞填充
            if bShow
                figure,imshow(BW),title('最后处理的有种子的图片')
            end
% % % % % % % % % % % % % % % %           
        end
        
        %% 以下是改进版本
        function [BW, data, tt] = ExtractSeedMask2(img8, bShow, mmPerPixel, args)
            BW = Helper.ExtractSeeds(img8, false);%%提取种子的完整形态，并去除噪声。
            
% %             w = 1;
% %             BW = Helper.DoMorphByW(BW, w);
            
            if bShow
                figure, imshow(BW),title('分割结果')
                
                BBB = imoverlay(img8, bwperim(BW, 8), 'red');
                figure,montage({img8, BBB}, 'Size', [1, 2]),title('种子的边界分割')
            end            
            
            stats = regionprops('table', BW, 'Area', 'MajorAxisLength', 'MinorAxisLength', 'Centroid', 'Orientation', 'BoundingBox');
            
            centroid = stats.Centroid;
            majLength = stats.MajorAxisLength;
            minLength = stats.MinorAxisLength;
            orientation = stats.Orientation;
            bboxes = stats.BoundingBox;
            
            [ins, ~, ~] = Helper.findIndexForSeed(stats);%%定义好种子的编号，ins(i) 表示第i-th图像种子序号对应的 数据的索引位置
            
% %             if bShow  %%是否画图 ，不再判断,一定画图
                tt = Helper.showBboxAndCentreOnFigure(img8, ins, centroid, orientation, majLength, minLength);
% %             end            
            
            % % % % % % % % % %计算新的短轴与长轴
            args.debug = false;
            args.debug = true;
            args.debug = false;
            args.mmPerPixel = mmPerPixel;
            [newMinor, newMajor] = Helper.getNewMinorRegionOfSeed(BW, ins, centroid, ...
                                               orientation, majLength, minLength, bboxes, args);            
          
            % % % % % % % %
% % %             newMinor2 = min(newMinor(:, 3)-2, minLength);%%%老用法
% % %             newMajor2 = majLength + 1;%%老用法
% % % % %             newMinor2 = newMinor(:, 3) - 1;
% % % % %             newMajor2 = newMajor(:, 3) - 1;%% 2022-01-14 统一采用这个估计
            newMinor2 = newMinor(:, 5);
            newMajor2 = newMajor(:, 5);%% 2022-01-15
%             newMinor2 = newMinor(:, 6);
%             newMajor2 = newMajor(:, 6);%% 2022-01-17
            
            
            %             data = [(1:length(ins))',  majLength(ins) * mmPerPixel, newMinor(ins, 3) * mmPerPixel];
            data = [(1:length(ins))',  newMajor2(ins) * mmPerPixel, newMinor2(ins) * mmPerPixel];
% %             if bShow
% %                 data
% %             end
            
        end
        
        %% 计算新的长短轴,具体每一个种子，计算上、下半轴
        function [newMinor, newMajor] = getNewMinorRegionOfSeed(BW, ins,centroid, orientation, ...
                                                                majLength, minLength, bboxes, args)
            if args.debug
                figure
            end
            newMinor = zeros(length(minLength), 4,6);%%（径向最大值，径向最小值，直径计算，原box直径,第k个最大直径，Feret直径）
            newMinor(:, 4) = minLength;%%保存
            newMajor = zeros(length(majLength), 4,6);
            newMajor(:, 4) = majLength;%%保存

            kk = 3;%%第kk = 3 的最值
            rr = 0.002;%%选取最值比例，晚一点用
            fig = 0;
            for index = 1 : length(ins)
                i = ins(index);%%真实序号index 所对应数组的位置
                
                major =  majLength(i);%%长轴
                minor = minLength(i);%%短轴
                %                 cent = centroid(i, :);%%中心
                theta = orientation(i);%%注意是角度，不是弧度
                box = bboxes(i, :);
                cent = centroid(i, :);
                
                
% % % % % % % % % % % % % % % % % % % % % % % % % 原来取子区域的做法               
                w = 3;
                box(1:2) = box(1:2) - w;
                box(3:4) = box(3:4)  + w*2 + 1;
                box = floor(box);
                sub = BW(box(2) : box(2) + box(4), box(1) : box(1) + box(3));%%取出局部区域
% % % % % % % % % % % % % % % % % % % % % % % % % %                 
                
                statSub = regionprops('table', sub, 'Centroid', 'PixelList');%%以后可以优化，不需要在这里处理
                pxyList = statSub.PixelList{1};%%节点列表
                % %                 cent2 = centroid(i, :) - box(1:2) + 1;
                vec = [-sind(-theta); cosd(-theta)];%%记着是 -theta
                dd = pxyList * vec;
                d0 = statSub.Centroid * vec;
                newMinor(i, 1:2) = [max(dd) - d0, min(dd) - d0];
                newMinor(i, 3) = newMinor(i, 1) - newMinor(i, 2);
                
                % %                 newMinor(i, :)
                
                vec2 = [cosd(-theta); sind(-theta)];
                ee = pxyList * vec2;
                e0 = statSub.Centroid * vec2;
                newMajor(i, 1:2) = [max(ee) - e0, min(ee) - e0];
                newMajor(i, 3) = newMajor(i, 1) - newMajor(i, 2);
                
                
                % % % % % % % % % % % % % % % % % % % 根据前kk个来计算
                ddkx = maxk(dd, kk);
                ddkn = mink(dd, kk);
                %%newMinor(i, 5) = ddkx(kk) - ddkn(kk);
                newMinor(i, 5) = mean(ddkx) - mean(ddkn);
                
                eekx = maxk(ee, kk);
                eekn = mink(ee, kk);
                %%newMajor(i, 5) = eekx(kk) - eekn(kk);    %%2022-01-15
                newMajor(i, 5) = mean(eekx) - mean(eekn);  %%2022-01-16
                
                % % % % % % % % % % % % % % % % % % %
                % % % % % % % % % % % % % % % %Feret直径
                out = bwferet(sub, 'all');
                newMinor(i, 6) = out.MinDiameter(1);%%取第一个
                newMajor(i, 6) = out.MaxDiameter(1);                
                
                % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
                % % % % % % % % % 进行直方图统计
% % %                 figure,imshow(sub),title('原图')
% % %                 
% % % % %                 [N,edges] = histcounts(dd-d0)
                if exist('args','var') == 1  && isfield(args, 'saveImage') && args.saveImage == true                  
                    if fig == 0
                        fig = figure;
                    else
                        figure(fig);
                        clf
                    end
                    str1 = sprintf('短(%.1f, %.1f)', newMinor(i, 2), newMinor(i, 1));
                    str2 = sprintf('长(%.1f, %.1f)', newMajor(i, 2), newMajor(i, 1));
                    str3 = sprintf('%d, %.1f\n(%.2f, %.2f)\n(%.2f, %.2f, %.2f)\n(%.2f, %.2f)\n(%.2f, %.2f)', ...
                                                    sum(sum(sub)), sum(sum(sub)) * 0.005,...
                                                    newMinor(i, 3) * args.mmPerPixel, ...
                                                    newMajor(i, 3) * args.mmPerPixel, ...
                                                   (newMinor(i, 3) -2) * args.mmPerPixel, ...
                                                   newMinor(i, 5) * args.mmPerPixel, newMajor(i, 5) * args.mmPerPixel, ...
                                                   newMinor(i, 3) - newMinor(i, 5),  newMajor(i, 3)- newMajor(i, 5), ...
                                                   newMinor(i,6) * args.mmPerPixel, newMajor(i, 6) * args.mmPerPixel ...
                                                   );

                    edged = newMinor(i, 2) : newMinor(i, 1);
                    edgee = newMajor(i, 2) : newMajor(i, 1);
                    subplot(1,3,1),histogram(dd-d0, edged, 'normalization', 'probability'),title(str1)
                    subplot(1,3,2),histogram(ee-e0, edgee, 'normalization', 'probability'),title(str2)
                    subplot(1,3,3),imshow(sub),title(str3)
                    fileN = sprintf('%ssub\\s_%s-%2d.png', args.DataPath, args.ImgName, index);
                    saveas(fig, fileN)
                end
                % % % % % % % % % % %                
                % % % % % % % % % % % % % 
                % %                 newMinor(i, 3) = newMinor(i, 3) * newMajor(i, 4)/newMajor(i, 3);%%尝试调整比例，OK
                % %                 newMinor(i, 3) = newMinor(i, 3) -2;%%直接减去2个像素，更有效一些，这里不减，留待外面减%%2022-01-14
                % % % % % % % % % % % % % % % % %
                
                if args.debug
                    [maxd, ins1] = max(dd);
                    [mind, ins2] = min(dd);
                    [maxe, ins3] = max(ee);
                    [mine, ins4] = min(ee);
                    imshow(sub)
                    hold on
                    Helper.drawBoxAndLine(statSub.Centroid , major, minor, theta)
                    % % %                 %%%%% 验证画出的直线
                    % % %                 xx = [1, box(3)];
                    % % %                 yy = tand(-theta) * xx + d0/cosd(-theta);
                    % % %                 line(xx, yy, 'color', 'm', 'LineStyle', '--' );
                    % % %                 %%%%%
                    
                    % % % % % % % % % % % % % % % %
                    ptxx = [pxyList(ins1, 1); pxyList(ins2, 1)];
                    ptyy = [pxyList(ins1, 2); pxyList(ins2, 2)];
                    line(ptxx, ptyy, 'color', 'r');
                    
                    ptxx = [pxyList(ins3, 1); pxyList(ins4, 1)];
                    ptyy = [pxyList(ins3, 2); pxyList(ins4, 2)];
                    line(ptxx, ptyy, 'color', 'k');
                    
                    plot(statSub.Centroid(1), statSub.Centroid(2), 'xb')
                    hold off
                end
            end            
        end
        %%%%% 2022-01-17计算更准确的数值  效果不明显，没写完
        function sub = getExactRegion(BW, box, args)
            widht = box(3) - box(1);
            height = box(4) - box(2);
            w = (widht + height)/2;
            box(1:2) = box(1:2) - w;
            box(3:4) = box(3:4)  + w*2 + 1;
            box = floor(box);
            
            gray = args.gray;
            local = imcrop(gray, box);
            BW2 = local;
            
            sub = BW(box(2) : box(2) + box(4), box(1) : box(1) + box(3));%%取出局部区域
            
        end
        
        
        %% 画出种子的编号图片，以及bounding box, 以及重心，长宽等
        function tt = showBboxAndCentreOnFigure(img8, ins, centroid, orientation, majLength, minLength)
            mm = max(majLength);
% %             figure, image(img8),tt = title('编号结果');
            figure('Name','种子的分割示意图','NumberTitle','off');
            image(img8),tt = title('编号结果');
            hold on
            for i = 1 : length(ins)
                index = ins(i);
                xx = centroid(index, 1);
                yy = centroid(index, 2) - mm;
                text(xx, yy, num2str(i), 'FontSize', 14, 'Color', 'red');
            end
            hold off
            
            hold on  %%画出种子的外接矩形(带旋转角度)
            for i = 1 : length(ins)
                index = ins(i);
                major =  majLength(index);%%长轴
                minor = minLength(index);%%短轴
                cent = centroid(index, :);%%中心
                theta = orientation(index);
                
                Helper.drawBoxAndLine(cent, major, minor, theta);
                
            end
            plot(centroid(:, 1), centroid(:,2), 'xb', 'MarkerSize', 8);
            hold off
        end
        %%画出旋转的矩形框，与长短轴
        function drawBoxAndLine(cent, major, minor, theta)
            bbox = [cent - [major/2, minor/2], major, minor];%%（左上角， 长，宽）
            conners = bbox2points(bbox);%%矩形的4个角点坐标
            
            
            [x, y] = Helper.rotateByDegreeOffset(conners(:, 1), conners(:, 2), theta, cent);%%旋转
            
            x = [x; x(1)];%%形成封闭曲线
            y = [y; y(1)];
            line(x, y, 'LineWidth', 2)
            
            xx = repmat(cent(1), 2, 1) + [-major/2; major/2];
            yy = repmat(cent(2), 2, 1);
            [x, y] = Helper.rotateByDegreeOffset(xx, yy, theta, cent);%%旋转
            line(x, y, 'LineWidth', 1, 'color', 'g');
            
            xx = repmat(cent(1), 2, 1) ;
            yy = repmat(cent(2), 2, 1) + [-minor/2; minor/2];
            [x, y] = Helper.rotateByDegreeOffset(xx, yy, theta, cent);%%旋转
            line(x, y, 'LineWidth', 1, 'color', 'g');
        end
        
        
        
        %% 按指定坐标逆时针旋转 角度
        function [x, y] = rotateByDegreeOffset(xx, yy, theta, offset)
            tform = affine2d([cosd(-theta) sind(-theta) 0;...
                -sind(-theta) cosd(-theta) 0;
                0           0 1]);
            
            [x,y] = transformPointsForward(tform, xx - offset(1), yy - offset(2));
            x = x + offset(1);
            y = y + offset(2);
        end
        
        %%根据种子的x-y坐标，进行编号。
        function [pos, xx, yy] = findIndexForSeed(stats)
            centroid = stats.Centroid;
            majLength = stats.MajorAxisLength;
            minLength = stats.MinorAxisLength;
            
            medLength = median(majLength);
            
            % % % % % %             zz = centroid(:, 1) + 9 *  centroid(:, 2);
            % % % % % %             [vv, ins]= sort(zz);%%没用了
            
            xx = centroid(:, 1);
            yy = centroid(:, 2);
            
            pos = 1 : length(xx);
            for i = 1 : length(pos) -1
                
                for j = i+1 : length(pos)
                    if yy(j) < yy(i) - 1.5 * medLength
                        tmp = xx(i);
                        xx(i) = xx(j);
                        xx(j) = tmp;
                        
                        tmp = yy(i);
                        yy(i) = yy(j);
                        yy(j) = tmp;
                        
                        tmp = pos(i);
                        pos(i) = pos(j);
                        pos(j) = tmp;
                        
                    elseif xx(i) > xx(j) && abs(yy(j) - yy(i)) <= 1.5 * medLength
                        tmp = xx(i);
                        xx(i) = xx(j);
                        xx(j) = tmp;
                        
                        tmp = yy(i);
                        yy(i) = yy(j);
                        yy(j) = tmp;
                        
                        tmp = pos(i);
                        pos(i) = pos(j);
                        pos(j) = tmp;
                    end
                end
                %                 pos
            end
            
        end
        
        
    end
end

